﻿using System.Data;
using System.Globalization;
using System.Runtime.InteropServices;
using TPXSDK.NET.Link;
using TPXSDK.NET.Share;

namespace TPXSDK.NET.Log;

/// <summary>
/// Tlm日志数据结构
/// </summary>
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
public struct LogEntryTlm
{
    #region 超集数据
    public uint TS;     // timestamp for camera snap in ms
    public float Roll;
    public float Pitch;
    public double Lat;
    public double Lng;
    #endregion

    public static DataTable Load(Stream fs, LogParserBase? parser = null)
    {
        DataRow? prevDataRow  = null;
        var dataTab = DataUtils.Reflect2DataTable<LogEntryTlm>("TlmLogEntry");
        using var sr = new StreamReader(fs);
        while (!sr.EndOfStream)
        {
            var line = sr.ReadLine();
            if (string.IsNullOrEmpty(line)) continue;
            var segs = line.Split(' ', StringSplitOptions.RemoveEmptyEntries);
            if (segs.Length < 3) continue;

            var utc = uint.Parse(segs[0], NumberStyles.AllowHexSpecifier);
            if (!Enum.TryParse(segs[1], out TlmType tlmType))
            {
                continue;
            }

            try
            {
                var rawData = Convert.FromBase64String(segs[2]);
                var dr = dataTab.NewRow();
                if (prevDataRow != null)
                {
                    for(var i =0; i< dataTab.Columns.Count; i++) dr[i] = prevDataRow[i];
                }
                dr["TS"] = utc;//更新时间戳
                FillRowData(tlmType, rawData, dr);
                dataTab.Rows.Add(dr);
                parser?.Report(dataTab.Rows.Count);
                prevDataRow = dr;
            }
            catch
            {
                // Ignore invalid TLM data in Base64 Format
            }
        }

        return dataTab;
    }

    #region 超集数据(TLM0)
    public float Speed;
    public float NavSpd;
    public int RCState;
    public int FlightMode;
    public int FlightStage;
    public float BaroAlt;
    public float AGL;
    public float Bat1Volt;
    public float Bat2Volt;
    #endregion

    #region 超集数据(TLM1)
    public int BestGPS;
    public int GPS1SatCnt;
    public int GPS1Status;
    public int GPS1HDOP;

    public int GPS2SatCnt;
    public int GPS2Status;
    public int GPS2HDOP;

    public float GndSpeed;
    public float GPSAlt;
    public float WindDir;
    public float WindSpd;
    #endregion

    #region 超集数据(TLM2)
    public int SRV1;
    public int SRV2;
    public int SRV3;
    public int SRV4;
    public int SRV5;
    public int SRV6;
    public int SRV7;
    public int SRV8;
    public int RCRoll;
    public int RCPitch;
    public int RCYaw;
    public int RCThr;
    #endregion

    #region 超集数据(TLM3)
    public float Yaw;
    public float NavYaw;
    public float Course;
    public float NavCourse;
    public float NavAlt;
    public int Aileron;
    public int Elevator;
    public int Rudder;
    public int Throttle;
    public int CmdStep;
    #endregion

    #region 超集数据(TLM4)
    public float NavPitch;
    public float NavRoll;
    public float Alt;
    public float AltDot;
    public float ClimbState;
    public float Voyage;
    public bool IsReady;
    public bool IsOffGnd;
    public bool IsLocked;
    public bool IsVibeWarn;
    public bool IsLogOk;
    public bool IsBatFail;
    public int Temp;
    #endregion

    private static void FillRowData(TlmType tlmType, byte[] rawData, DataRow dr)
    {
        if (tlmType == TlmType.STD0)
        {
            var tlm = DataUtils.BytesToStruct<STD_TLM_0>(rawData);
            dr["Roll"] = tlm.Pitch * 0.01f;
            dr["Pitch"] = tlm.Roll * 0.01f;
            dr["Lng"] = tlm.Lng / 1e7;
            dr["Lat"] = tlm.Lat / 1e7;

            dr["Speed"]   = tlm.Speed * 0.01f;
            dr["NavSpd"]  = tlm.TargetSpd * 0.01f;
            dr["RCState"] = tlm.RCState;
            dr["FlightMode"] = tlm.FlightMode >> 4;
            dr["FlightStage"] = tlm.FlightMode & 0x0F;

            dr["BaroAlt"] = tlm.BaroAlt * 0.01f;
            dr["AGL"] = tlm.AGL * 0.01f;// 转换为m

            // 电池电压[V]
            dr["Bat1Volt"] = 0.01f * tlm.Volt1;
            dr["Bat2Volt"] = 0.01f * tlm.Volt2;

        }
        else if (tlmType == TlmType.STD1)
        {
            var tlm = DataUtils.BytesToStruct<STD_TLM_1>(rawData);
            dr["Roll"] = tlm.Pitch * 0.01f;
            dr["Pitch"] = tlm.Roll * 0.01f;
            dr["Lng"] = tlm.Lng / 1e7;
            dr["Lat"] = tlm.Lat / 1e7;

            dr["BestGPS"] = tlm.BestGPS;
            dr["GPS1SatCnt"] = tlm.GPS1SatNum;
            dr["GPS1Status"] = tlm.GPS1Status;
            dr["GPS1HDOP"] = tlm.GPS1DOP;

            dr["GPS2SatCnt"] = tlm.GPS2SatNum;
            dr["GPS2Status"] = tlm.GPS2Status;
            dr["GPS2HDOP"] = tlm.GPS2DOP;

            dr["GndSpeed"] = tlm.GndSpd * 0.01f;  // 地速:m/s
            dr["GPSAlt"]   = tlm.GPSAlt * 0.01f;  // 海拔:meter
            dr["WindDir"]  = tlm.WindDir * 360.0f / 256.0f;      // 风向:0~360°
            dr["WindSpd"]  = tlm.WindSpd * 0.01f;  // 风速:m/s
        }
        else if (tlmType == TlmType.STD2)
        {
            var tlm = DataUtils.BytesToStruct<STD_TLM_2>(rawData);
            dr["Roll"] = tlm.Pitch * 0.01f;
            dr["Pitch"] = tlm.Roll * 0.01f;
            dr["Lng"] = tlm.Lng / 1e7;
            dr["Lat"] = tlm.Lat / 1e7;

            // 伺服输出, 范围 0~200，转换为 1000~2000 us
            dr["SRV1"] = 1000 + 5 * tlm.SRV1;
            dr["SRV2"] = 1000 + 5 * tlm.SRV2;
            dr["SRV3"] = 1000 + 5 * tlm.SRV3;
            dr["SRV4"] = 1000 + 5 * tlm.SRV4;
            dr["SRV5"] = 1000 + 5 * tlm.SRV5;
            dr["SRV6"] = 1000 + 5 * tlm.SRV6;
            dr["SRV7"] = 1000 + 5 * tlm.SRV7;
            dr["SRV8"] = 1000 + 5 * tlm.SRV8;

            // RC遥控器输出, 范围 -100~100
            dr["RCRoll"]  = tlm.RCR;
            dr["RCPitch"] = tlm.RCP;
            dr["RCYaw"]   = tlm.RCY;
            dr["RCThr"]   = tlm.RCT;
        }
        else if (tlmType == TlmType.STD3)
        {
            var tlm = DataUtils.BytesToStruct<STD_TLM_3>(rawData);
            dr["Roll"] = tlm.Pitch * 0.01f;
            dr["Pitch"] = tlm.Roll * 0.01f;
            dr["Lng"] = tlm.Lng / 1e7;
            dr["Lat"] = tlm.Lat / 1e7;

            dr["Yaw"]       = tlm.Yaw * 360.0f / 256.0f;
            dr["NavYaw"]    = tlm.TargetYaw * 360.0f / 256.0f;
            dr["Course"]    = tlm.Hdg * 360.0f / 256.0f;
            dr["NavCourse"] = tlm.TargetHdg * 360.0f / 256.0f;
            dr["NavAlt"]    = tlm.TargetAlt * 0.01f;
            dr["Aileron"]   = tlm.Aileron;
            dr["Elevator"]  = tlm.Elevator;
            dr["Rudder"]    = tlm.Rudder;
            dr["Throttle"]  = tlm.Throttle;
            dr["CmdStep"]   = tlm.CmdStep;
        }
        else if (tlmType == TlmType.STD4)
        {
            var tlm = DataUtils.BytesToStruct<STD_TLM_4>(rawData);
            dr["Roll"] = tlm.Pitch * 0.01f;
            dr["Pitch"] = tlm.Roll * 0.01f;
            dr["Lng"] = tlm.Lng / 1e7;
            dr["Lat"] = tlm.Lat / 1e7;

            dr["NavPitch"]   = tlm.TargetPitch * 0.01f;
            dr["NavRoll"]    = tlm.TargetRoll * 0.01f;
            dr["Alt"]        = tlm.Alt * 0.01f;
            dr["AltDot"]     = tlm.AltDot * 0.01f;
            dr["ClimbState"] = tlm.ClimbState;
            dr["Voyage"]     = tlm.Voyage;// 总航程
            dr["IsReady"]    = DataUtils.BIT16_TST(tlm.Flags, 0);
            dr["IsOffGnd"]   = DataUtils.BIT16_TST(tlm.Flags, 1);
            dr["IsLocked"]   = DataUtils.BIT16_TST(tlm.Flags, 2);
            dr["IsVibeWarn"] = DataUtils.BIT16_TST(tlm.Flags, 3);
            dr["IsLogOk"]    = DataUtils.BIT16_TST(tlm.Flags, 4);
            dr["IsBatFail"]  = DataUtils.BIT16_TST(tlm.Flags, 5);
            dr["IsAvoiding"] = DataUtils.BIT16_TST(tlm.Flags, 6);
            dr["IsVNSUsed"]  = DataUtils.BIT16_TST(tlm.Flags, 7);
            dr["IsVNSRdy"]   = DataUtils.BIT16_TST(tlm.Flags, 8);

            dr["Temp"]       = tlm.Temp;
        }
    }
}
